﻿#pragma once
#include <boost/asio.hpp>
#include <iostream>
#include <iomanip>
#include <memory>
#include <vector>
#include "pelco_d_smooth_128ms_5s.hpp"

namespace asio = boost::asio;
using asio::ip::tcp;

struct PelcoDFrame
{
    uint8_t eoi = 0xFF;
    uint8_t addr = 0x01;
    uint8_t cmd1 = 0x00, cmd2 = 0x00;
    uint8_t data1 = 0x00, data2 = 0x00;
    uint8_t data3 = 0x00, data4 = 0x00;
    uint8_t cksum = 0x00;
    uint8_t need = 0x00;
    uint8_t cksum7() {
        cksum = (addr + cmd1 + cmd2 + data1 + data2) & 0xFF;
        need = 7;
        return cksum;
    }
    uint8_t cksum9() {
        cksum = (addr + cmd1 + cmd2 + data1 + data2 + data3 + data4) & 0xFF;
        need = 9;
        return cksum;
    }
    std::vector<uint8_t> to_bytes() {
        return need == 7 ?
            std::vector<uint8_t>{ eoi, addr, cmd1, cmd2, data1, data2, cksum } :
            std::vector<uint8_t>{ eoi, addr, cmd1, cmd2, data1, data2, data3, data4, cksum };
    }
};

class PelcoDSmoothServer : public std::enable_shared_from_this<PelcoDSmoothServer>
{
public:
    explicit PelcoDSmoothServer(unsigned short port)
        : acceptor_(ioc_, tcp::endpoint{ tcp::v4(), port }) {
    }

    ~PelcoDSmoothServer() {
        pan_smooth_.stop();
        tilt_smooth_.stop();
    }

    void run() {
        accept_next();
        ioc_.run();
    }

    void stop() { ioc_.stop(); }

private:
    void accept_next() {
        new_sock_ = std::make_unique<tcp::socket>(ioc_);
        acceptor_.async_accept(*new_sock_,
            [self = shared_from_this()]
            (const boost::system::error_code& ec)
            {
                if (!ec) self->on_connected();
                else     self->accept_next();
            });
    }

    void on_connected() {
        sock_ = std::move(new_sock_);
        std::cout << "Pelco-D smooth client: " << sock_->remote_endpoint() << '\n';
        acceptor_.cancel();
        read_loop();
    }

    void read_loop() {
        asio::async_read(*sock_, asio::dynamic_buffer(rb_),
            asio::transfer_at_least(1),
            [self = shared_from_this()]
            (const boost::system::error_code& ec, std::size_t /*bytes_transferred*/)
            {
                if (ec) { self->on_disconnected(); return; }
                self->scan_frames();
                self->read_loop();
            });
    }

    void scan_frames() {
        printf("RX ");
        for (size_t i = 0; i < rb_.size(); i++) {
            printf(i == rb_.size() - 1 ? "%02X\n" : "%02X ", rb_[i]);
        }
        while (true) {
            auto it = std::find(rb_.begin(), rb_.end(), 0xFF);
            if (it == rb_.end()) {
                rb_.clear();
                break;
            }
            if (it != rb_.begin()) rb_.erase(rb_.begin(), it);
            if (rb_.size() < 2) break;
            uint8_t cmd2 = static_cast<uint8_t>(rb_[3]);
            std::size_t need = (cmd2 == 0xF9) ? 9 : 7;
            if (rb_.size() < need) break;
            std::vector<uint8_t> pkt(rb_.begin(), rb_.begin() + need);
            PelcoDFrame f;
            f.addr = pkt[1]; f.cmd1 = pkt[2]; f.cmd2 = pkt[3];
            f.data1 = pkt[4]; f.data2 = pkt[5];
            bool ok = false;
            if (need == 7) {
                ok = (pkt[6] == f.cksum7());
                if (ok) handle7(f);
            }
            else {
                f.data3 = pkt[6]; f.data4 = pkt[7];
                ok = (f.cksum9() == pkt[8]);
                if (ok) handle9(f);
            }
            if (ok) rb_.erase(rb_.begin(), rb_.begin() + need);
            else    rb_.erase(rb_.begin());
        }
    }

    void handle7(const PelcoDFrame& f) {
        // 获取水平位置
        if (f.cmd1 == 0x00 && f.cmd2 == 0x51) {
            send_pan_pos_reply(f.addr);
        }
        // 获取俯仰位置
        else if (f.cmd1 == 0x00 && f.cmd2 == 0x53) {
            send_tilt_pos_reply(f.addr);
        }
        // 获取水平俯仰位置
        else if (f.cmd1 == 0x00 && f.cmd2 == 0xF7) {
            send_pos_reply(f.addr);
        }
        // 设置水平位置
        else if (f.cmd1 == 0x00 && f.cmd2 == 0x4B) {
            // 平滑变动到目标位置 历经5秒，5000/128步
            uint16_t pan = (static_cast<uint16_t>(f.data1) << 8) | f.data2;
            std::cout << "Set smooth target pan=" << pan << '\n';
            pan_smooth_.set_target(pan, [this](uint16_t v) { current_pan_.store(v); });
        }
        // 设置俯仰位置
        else if (f.cmd1 == 0x00 && f.cmd2 == 0x4D) {
            // 平滑变动到目标位置 历经5秒，5000/128步
            uint16_t tilt = (static_cast<uint16_t>(f.data1) << 8) | f.data2;
            std::cout << "Set smooth target tilt=" << tilt << '\n';
            tilt_smooth_.set_target(tilt, [this](uint16_t v) { current_tilt_.store(v); });
        }
    }

    void handle9(const PelcoDFrame& f) {
        // 设置云台水平俯仰位置
        if (f.cmd1 == 0x00 && f.cmd2 == 0xF9) {
            // 平滑变动到目标位置 历经5秒，5000/128步
            uint16_t pan = (static_cast<uint16_t>(f.data1) << 8) | f.data2;
            uint16_t tilt = (static_cast<uint16_t>(f.data3) << 8) | f.data4;
            std::cout << "Set smooth target pan=" << pan << " tilt=" << tilt << '\n';
            pan_smooth_.set_target(pan, [this](uint16_t v) { current_pan_.store(v); });
            tilt_smooth_.set_target(tilt, [this](uint16_t v) { current_tilt_.store(v); });
        }
    }

    void send_pan_pos_reply(uint8_t addr) {
        PelcoDFrame f;
        f.addr = addr;
        f.cmd1 = 0x00; f.cmd2 = 0x59;
        uint16_t pan = current_pan_.load();
        f.data1 = (pan >> 8) & 0xFF; f.data2 = pan & 0xFF;
        f.cksum7();
        send(std::make_shared<std::vector<uint8_t>>(f.to_bytes()));
    }

    void send_tilt_pos_reply(uint8_t addr) {
        PelcoDFrame f;
        f.addr = addr;
        f.cmd1 = 0x00; f.cmd2 = 0x5B;
        uint16_t tilt = current_tilt_.load();
        f.data1 = (tilt >> 8) & 0xFF; f.data2 = tilt & 0xFF;
        f.cksum7();
        send(std::make_shared<std::vector<uint8_t>>(f.to_bytes()));
    }

    void send_pos_reply(uint8_t addr) {
        PelcoDFrame f;
        f.addr = addr;
        f.cmd1 = 0x00; f.cmd2 = 0xF5;
        uint16_t pan = current_pan_.load();
        uint16_t tilt = current_tilt_.load();
        f.data1 = (pan >> 8) & 0xFF; f.data2 = pan & 0xFF;
        f.data3 = (tilt >> 8) & 0xFF; f.data4 = tilt & 0xFF;
        f.cksum9();
        send(std::make_shared<std::vector<uint8_t>>(f.to_bytes()));
    }

    void send(std::shared_ptr<std::vector<uint8_t>> buf) {
        asio::async_write(*sock_, asio::buffer(*buf),
            [self = shared_from_this(), buf]
            (const boost::system::error_code& ec, std::size_t bytes_transferred) {
                if (!ec) {
                    printf("TX ");
                    for (size_t i = 0; i < bytes_transferred; i++) {
                        printf(i == bytes_transferred - 1 ? "%02X\n" : "%02X ", (*buf)[i]);
                    }
                }
            });
    }

    void on_disconnected() {
        std::cout << "Client disconnected. Wait next...\n";
        sock_.reset();
        rb_.clear();
        accept_next();
    }

    asio::io_context ioc_;
    tcp::acceptor acceptor_;
    std::unique_ptr<tcp::socket> new_sock_;
    std::unique_ptr<tcp::socket> sock_;
    std::vector<uint8_t> rb_;

    SmoothValue<uint16_t> pan_smooth_;
    SmoothValue<uint16_t> tilt_smooth_;

    std::atomic<uint16_t> current_pan_{ 0 };
    std::atomic<uint16_t> current_tilt_{ 0 };
};